// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//  »Project«   Talina Gaming System (TgS) (∂)
//  »File«      TgS Common - Math API [Vector] [F] [S].i_inc
//  »Author«    Andrew Aye (EMail: mailto:andrew.aye@gmail.com, Web: http://www.andrewaye.com)
//  »Version«   4.0
// ------------------------------------------------------------------------------------------------------------------------------ //
//  Copyright: © 2002-2010, Andrew Aye.  All Rights Reserved.
//  This software is free for non-commercial use. Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met: 
//    Redistributions of source code must retain this copyright notice, this list of conditions and the following disclaimers. 
//    Redistributions in binary form must reproduce this copyright notice, this list of conditions and the following
//      disclaimers in the documentation and other materials provided with the distribution. 
//  Neither the names of the copyright owner nor the names of its contributors may be used to endorse or promote products derived
//  from this software without specific prior written permission. 
//  The intellectual property rights of the algorithms used reside with Andrew Aye.  You may not use this software, in whole or
//  in part, in support of any commercial product without the express written consent of the author.
//  There is no warranty or other guarantee of fitness of this software for any purpose. It is provided solely "as is".
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //

// ---- VECTOR - VALIDATION ----------------------------------------------------------------------------------------------------- //

TgINLINE TgBOOL V3(F_Is_Point_Valid)( V3(CPCU_TgVEC) ptv0 )
{
    return ( !F(tgCM_NaN)( ptv0->m_aData[0] ) && !F(tgCM_NaN)( ptv0->m_aData[1] ) && !F(tgCM_NaN)( ptv0->m_aData[2] ) );
}


TgINLINE TgBOOL V4(F_Is_Point_Valid)( V4(CPCU_TgVEC) ptv0 )
{
    return ( !F(tgCM_NaN)( ptv0->m_aData[0] ) && !F(tgCM_NaN)( ptv0->m_aData[1] ) &&
        !F(tgCM_NaN)( ptv0->m_aData[2] ) && (MKL(1.0) == ptv0->m_aData[3]) );
}


TgINLINE TgBOOL V3(F_Is_Vector_Valid)( V3(CPCU_TgVEC) ptv0 )
{
    return ( !F(tgCM_NaN)( ptv0->m_aData[0] ) && !F(tgCM_NaN)( ptv0->m_aData[1] ) && !F(tgCM_NaN)( ptv0->m_aData[2] ) );
}


TgINLINE TgBOOL V4(F_Is_Vector_Valid)( V4(CPCU_TgVEC) ptv0 )
{
    return ( !F(tgCM_NaN)( ptv0->m_aData[0] ) && !F(tgCM_NaN)( ptv0->m_aData[1] ) && 
        !F(tgCM_NaN)( ptv0->m_aData[2] ) && (MKL(0.0) == ptv0->m_aData[3]) );
}




// ---- VECTOR 4 - INIT --------------------------------------------------------------------------------------------------------- //

TgINLINE V4(TgVEC) V4(F_SET_ELEM)( const TYPE tyX, const TYPE tyY, const TYPE tyZ, const TYPE tyW )
{
    V4(TgVEC)                          tvResult;

    tvResult.m_aData[0] = tyX;
    tvResult.m_aData[1] = tyY;
    tvResult.m_aData[2] = tyZ;
    tvResult.m_aData[3] = tyW;

    return (tvResult);
}


TgINLINE V4(TgVEC) V4(F_SETP_ELEM)( const TYPE tyX, const TYPE tyY, const TYPE tyZ )
{
    V4(TgVEC)                          tvResult;

    tvResult.m_aData[0] = tyX;
    tvResult.m_aData[1] = tyY;
    tvResult.m_aData[2] = tyZ;
    tvResult.m_aData[3] = MKL(1.0);

    return (tvResult);
}


TgINLINE V4(TgVEC) V4(F_SETV_ELEM)( const TYPE tyX, const TYPE tyY, const TYPE tyZ )
{
    V4(TgVEC)                          tvResult;

    tvResult.m_aData[0] = tyX;
    tvResult.m_aData[1] = tyY;
    tvResult.m_aData[2] = tyZ;
    tvResult.m_aData[3] = MKL(0.0);

    return (tvResult);
}


TgINLINE V4(TgVEC) V4(F_SETP)( V4(CPCU_TgVEC) ptv0 )
{
    V4(TgVEC)                          tvResult;

    tvResult.m_aData[0] = ptv0->m_aData[0];
    tvResult.m_aData[1] = ptv0->m_aData[1];
    tvResult.m_aData[2] = ptv0->m_aData[2];
    tvResult.m_aData[3] = MKL(1.0);

    return (tvResult);
}


TgINLINE V4(TgVEC) V4(F_SETV)( V4(CPCU_TgVEC) ptv0 )
{
    V4(TgVEC)                          tvResult;

    tvResult.m_aData[0] = ptv0->m_aData[0];
    tvResult.m_aData[1] = ptv0->m_aData[1];
    tvResult.m_aData[2] = ptv0->m_aData[2];
    tvResult.m_aData[3] = MKL(0.0);

    return (tvResult);
}




// ---- HOMOGENEOUS THREE ELEMENT DOT PRODUCT ----------------------------------------------------------------------------------- //

TgINLINE TYPE V4(F_DOT3_VV)( V4(CPCU_TgVEC) ptv0, V4(CPCU_TgVEC) ptv1 )
{
    return (ptv0->m_aData[0]*ptv1->m_aData[0] + ptv0->m_aData[1]*ptv1->m_aData[1] + ptv0->m_aData[2]*ptv1->m_aData[2] );
}


TgINLINE V4(TgVEC) V4(F_CX)( V4(CPCU_TgVEC) ptv0, V4(CPCU_TgVEC) ptv1 )
{
    return (V4(F_SETV_ELEM)( 
        ptv0->m_aData[1]*ptv1->m_aData[2] - ptv0->m_aData[2]*ptv1->m_aData[1],
        ptv0->m_aData[2]*ptv1->m_aData[0] - ptv0->m_aData[0]*ptv1->m_aData[2],
        ptv0->m_aData[0]*ptv1->m_aData[1] - ptv0->m_aData[1]*ptv1->m_aData[0]
    ));
}


TgINLINE V4(TgVEC) V4(F_UCX)( V4(CPCU_TgVEC) ptv0, V4(CPCU_TgVEC) ptv1 )
{
    V4(C_TgVEC)                        tvRet =  V4(F_CX)( ptv0, ptv1 );

    return (V4(F_NORM)( &tvRet ));
}


TgINLINE V4(TgVEC) V4(F_UCX_LEN)( PCU_TYPE ptyLength, V4(CPCU_TgVEC) ptv0, V4(CPCU_TgVEC) ptv1 )
{
    V4(C_TgVEC)                        tvRet =  V4(F_CX)( ptv0, ptv1 );

    return (V4(F_NORM_LEN)( ptyLength, &tvRet ));
}




// ---- QUATERNION - FUNCTIONS -------------------------------------------------------------------------------------------------- //

TgINLINE TgVOID V4(F_QT_MUL)( V4(PCU_TgVEC) ptvR0, V4(CPCU_TgVEC) ptqR1, V4(CPCU_TgVEC) ptqR2 )
{
    ptvR0->m_aData[0] = ptqR1->m_aData[3]*ptqR2->m_aData[0] + ptqR1->m_aData[0]*ptqR2->m_aData[3]
                      + ptqR1->m_aData[1]*ptqR2->m_aData[2] - ptqR1->m_aData[2]*ptqR2->m_aData[1];
    ptvR0->m_aData[1] = ptqR1->m_aData[3]*ptqR2->m_aData[1] + ptqR1->m_aData[1]*ptqR2->m_aData[3]
                      + ptqR1->m_aData[2]*ptqR2->m_aData[0] - ptqR1->m_aData[0]*ptqR2->m_aData[2];
    ptvR0->m_aData[2] = ptqR1->m_aData[3]*ptqR2->m_aData[2] + ptqR1->m_aData[2]*ptqR2->m_aData[3]
                      + ptqR1->m_aData[0]*ptqR2->m_aData[1] - ptqR1->m_aData[1]*ptqR2->m_aData[0];
    ptvR0->m_aData[3] = ptqR1->m_aData[3]*ptqR2->m_aData[3] - ptqR1->m_aData[0]*ptqR2->m_aData[0]
                      - ptqR1->m_aData[1]*ptqR2->m_aData[1] - ptqR1->m_aData[2]*ptqR2->m_aData[2];
}


TgINLINE TgVOID V4(F_QT_SLERP)( V4(PCU_TgVEC) ptvR0, const TYPE tydT, V4(CPCU_TgVEC) ptvR1, V4(CPCU_TgVEC) ptvR2 )
{
    TYPE                                tyCosA;
    V4(TgVEC)                           tvX0,tvX1,tvX2;
    TYPE                                tyF0, tyF1;

    if (tydT <= MKL(0.0))
    {
        *ptvR0 = *ptvR1;
        return;
    }
    else if (tydT >= MKL(1.0))
    {
        *ptvR0 = *ptvR2;
        return;
    }

    tyCosA = V4(F_DOT_VV)( ptvR1, ptvR2 );

    if (tyCosA < MKL(0.0))
    {
        tyCosA = -tyCosA;
        tvX0 = V4(F_NEG)( ptvR2 );
    }
    else
    {
        tvX0 = *ptvR2;
    };

    if ((MKL(1.0) - tyCosA) > F(TgEPS))
    {
        const TYPE                          tyAngle = F(tgPM_ACOS)( tyCosA );
        const TYPE                          tyInvSinA = MKL(1.0) / F(tgPM_SIN)( tyAngle );

        tyF0 = F(tgPM_SIN)( ( MKL(1.0) - tydT ) * tyAngle ) * tyInvSinA;
        tyF1 = F(tgPM_SIN)( tydT * tyAngle ) * tyInvSinA;
    }
    else
    {
        tyF0 = MKL(1.0) - tydT;
        tyF1 = tydT;
    }

    tvX1 = V4(F_MUL_VS)( ptvR1, tyF0 );
    tvX2 = V4(F_MUL_VS)( &tvX0, tyF1 );

    *ptvR0 = V4(F_ADD_VV)( &tvX1, &tvX2 );
}


TgINLINE TgVOID V4(F_QT_VECTOR_TO_VECTOR)( V4(PCU_TgVEC) ptvR0, V4(CPCU_TgVEC) ptvFrom, V4(CPCU_TgVEC) ptvTo )
{
    // If either the scale or the dot product are invalid this will cause a return of the zero quaternion.

    const TYPE                          tyFm_To = V4(F_DOT3_VV)( ptvFrom, ptvTo ); //« cos(θ) = A*B/|A||B|
    const TYPE                          tyScale = V4(F_LEN)( ptvFrom )*V4(F_LEN)( ptvTo );
    TYPE                                tyCosA = F(tgPM_FSEL)( tyScale - F(TgEPS), tyFm_To / tyScale, MKL(1.0) );
    V4(C_TgVEC)                         tvAxis = V4(F_UCX)( ptvFrom, ptvTo );
    const TYPE                          tySinHA = F(tgPM_SQRT)((MKL(1.0) - tyCosA)*MKL(0.5));
    const TYPE                          tyLimit = F(tgCM_CLP)( tyCosA, MKL(1.0), MKL(-1.0) ); 

    // Ensure that the result is within function range (floating point error)
    // Axis of rotation would be the vector perpendicular to both inputs, normalized if necessary
    // For an axis-angle, need to generate cos(θ/2) and sin(θ/2)
    //  Trig Identity: cos(2θ) = cos²(θ) - sin²(θ) = 2*cos²(θ) - 1 = 1 - 2*sin²(θ)
    //                 cos(θ/2) = √((1 + cos(θ))/2)

    ptvR0->m_aData[0] = tvAxis.m_aData[0] * tySinHA;
    ptvR0->m_aData[1] = tvAxis.m_aData[1] * tySinHA;
    ptvR0->m_aData[2] = tvAxis.m_aData[2] * tySinHA;
    ptvR0->m_aData[3] = F(tgPM_SQRT)((MKL(1.0) + tyLimit)*MKL(0.5));
}




// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  Scalar Function
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

// ---- VECTOR 3 - INIT --------------------------------------------------------------------------------------------------------- //

TgINLINE V3(TgVEC) V3(F_SET_ELEM)( const TYPE tyX, const TYPE tyY, const TYPE tyZ )
{
    V3(TgVEC)                          tvResult;

    tvResult.m_aData[0] = tyX;
    tvResult.m_aData[1] = tyY;
    tvResult.m_aData[2] = tyZ;

    return (tvResult);
}


TgINLINE V3(TgVEC) V3(F_SETP_ELEM)( const TYPE tyX, const TYPE tyY, const TYPE tyZ )
{
    V3(TgVEC)                          tvResult;

    tvResult.m_aData[0] = tyX;
    tvResult.m_aData[1] = tyY;
    tvResult.m_aData[2] = tyZ;

    return (tvResult);
}


TgINLINE V3(TgVEC) V3(F_SETV_ELEM)( const TYPE tyX, const TYPE tyY, const TYPE tyZ )
{
    V3(TgVEC)                          tvResult;

    tvResult.m_aData[0] = tyX;
    tvResult.m_aData[1] = tyY;
    tvResult.m_aData[2] = tyZ;

    return (tvResult);
}


TgINLINE V3(TgVEC) V3(F_SETP)( V3(CPCU_TgVEC) ptv0 )
{
    V3(TgVEC)                          tvResult;

    tvResult.m_aData[0] = ptv0->m_aData[0];
    tvResult.m_aData[1] = ptv0->m_aData[1];
    tvResult.m_aData[2] = ptv0->m_aData[2];

    return (tvResult);
}


TgINLINE V3(TgVEC) V3(F_SETV)( V3(CPCU_TgVEC) ptv0 )
{
    V3(TgVEC)                          tvResult;

    tvResult.m_aData[0] = ptv0->m_aData[0];
    tvResult.m_aData[1] = ptv0->m_aData[1];
    tvResult.m_aData[2] = ptv0->m_aData[2];

    return (tvResult);
}


TgINLINE TgVOID V3(F_INIT_Basis_From_Vector)( V3(PCU_TgVEC) ptvB0, V3(PCU_TgVEC) ptvB1, V3(CPCU_TgVEC) ptvA )
{
    const TYPE                          fXX_ZZ = ptvA->m_aData[0]*ptvA->m_aData[0] + ptvA->m_aData[2]*ptvA->m_aData[2];
    const TYPE                          fXZ = F(tgPM_SQRT)( fXX_ZZ );

    TgASSERT( V3(F_Is_Vector_Valid)( ptvA ) && F(tgCM_NR1)( V3(F_LSQ)( ptvA ) ) );

    if (F(tgCM_NR0)( fXZ ))
    {
        *ptvB0 = V3(TgKV_UNIT_X);
        *ptvB1 = V3(TgKV_UNIT_Z);
    }
    else
    {
        const TYPE                          fXY = ptvA->m_aData[0]*ptvA->m_aData[1];
        const TYPE                          fYZ = ptvA->m_aData[1]*ptvA->m_aData[2];
        const TYPE                          tyInvXZ = MKL(1.0) / fXZ;

        *ptvB0 = V3(F_SETV_ELEM)( -ptvA->m_aData[2] * tyInvXZ, MKL(0.0), ptvA->m_aData[0] * tyInvXZ );
        *ptvB1 = V3(F_SETV_ELEM)( -fXY, fXX_ZZ, -fYZ );
    };
}


TgINLINE V3(TgVEC) V3(F_Set_Ortho)( V3(CPCU_TgVEC) ptv0 )
{
    const TYPE                          fX = F(tgPM_ABS)( ptv0->m_aData[0] );
    const TYPE                          fY = F(tgPM_ABS)( ptv0->m_aData[1] );
    const TYPE                          fZ = F(tgPM_ABS)( ptv0->m_aData[2] );

    if (fX < fY && fX < fZ)
    {
        return (V3(F_SETV_ELEM)( MKL(0.0), fZ, -fY ));
    }
    else if (fY < fZ)
    {
        return (V3(F_SETV_ELEM)( fZ, MKL(0.0), -fX ));
    }
    else
    {
        return (V3(F_SETV_ELEM)( fY, -fX, MKL(0.0) ));
    };
}




// ---- VECTOR 4 - INIT --------------------------------------------------------------------------------------------------------- //

TgINLINE TgVOID V4(F_INIT_Basis_From_Vector)( V4(PCU_TgVEC) ptvB0, V4(PCU_TgVEC) ptvB1, V4(CPCU_TgVEC) ptvA )
{
    const TYPE                          fXX_ZZ = ptvA->m_aData[0]*ptvA->m_aData[0] + ptvA->m_aData[2]*ptvA->m_aData[2];
    const TYPE                          fXZ = F(tgPM_SQRT)( fXX_ZZ );

    TgASSERT( V4(F_Is_Vector_Valid)( ptvA ) && F(tgCM_NR1)( V4(F_LSQ)( ptvA ) ) );

    if (F(tgCM_NR0)( fXZ ))
    {
        *ptvB0 = V4(TgKV_UNIT_X);
        *ptvB1 = V4(TgKV_UNIT_Z);
    }
    else
    {
        const TYPE                          fXY = ptvA->m_aData[0]*ptvA->m_aData[1];
        const TYPE                          fYZ = ptvA->m_aData[1]*ptvA->m_aData[2];
        const TYPE                          tyInvXZ = MKL(1.0) / fXZ;

        *ptvB0 = V4(F_SETV_ELEM)( -ptvA->m_aData[2] * tyInvXZ, MKL(0.0), ptvA->m_aData[0] * tyInvXZ );
        *ptvB1 = V4(F_SETV_ELEM)( -fXY, fXX_ZZ, -fYZ );
    };
}


TgINLINE V4(TgVEC) V4(F_Set_Ortho)( V4(CPCU_TgVEC) ptv0 )
{
    const TYPE                          fX = F(tgPM_ABS)( ptv0->m_aData[0] );
    const TYPE                          fY = F(tgPM_ABS)( ptv0->m_aData[1] );
    const TYPE                          fZ = F(tgPM_ABS)( ptv0->m_aData[2] );

    if (fX < fY && fX < fZ)
    {
        return (V4(F_SETV_ELEM)( MKL(0.0), fZ, -fY ));
    }
    else if (fY < fZ)
    {
        return (V4(F_SETV_ELEM)( fZ, MKL(0.0), -fX ));
    }
    else
    {
        return (V4(F_SETV_ELEM)( fY, -fX, MKL(0.0) ));
    };
}




// ---- HOMOGENEOUS THREE ELEMENT DOT PRODUCT ----------------------------------------------------------------------------------- //

TgINLINE TYPE V3(F_DOT3_VV)( V3(CPCU_TgVEC) ptv0, V3(CPCU_TgVEC) ptv1 )
{
    return (ptv0->m_aData[0]*ptv1->m_aData[0] + ptv0->m_aData[1]*ptv1->m_aData[1] + ptv0->m_aData[2]*ptv1->m_aData[2] );
}


TgINLINE V3(TgVEC) V3(F_CX)( V3(CPCU_TgVEC) ptv0, V3(CPCU_TgVEC) ptv1 )
{
    return (V3(F_SETV_ELEM)( 
        ptv0->m_aData[1]*ptv1->m_aData[2] - ptv0->m_aData[2]*ptv1->m_aData[1],
        ptv0->m_aData[2]*ptv1->m_aData[0] - ptv0->m_aData[0]*ptv1->m_aData[2],
        ptv0->m_aData[0]*ptv1->m_aData[1] - ptv0->m_aData[1]*ptv1->m_aData[0]
    ));
}


TgINLINE V3(TgVEC) V3(F_UCX)( V3(CPCU_TgVEC) ptv0, V3(CPCU_TgVEC) ptv1 )
{
    V3(C_TgVEC)                        tvRet =  V3(F_CX)( ptv0, ptv1 );

    return (V3(F_NORM)( &tvRet ));
}


TgINLINE V3(TgVEC) V3(F_UCX_LEN)( TYPE * __restrict const ptyLength, V3(CPCU_TgVEC) ptv0, V3(CPCU_TgVEC) ptv1 )
{
    V3(C_TgVEC)                        tvRet =  V3(F_CX)( ptv0, ptv1 );

    return (V3(F_NORM_LEN)( ptyLength, &tvRet ));
}




// ---- QUATERNION - INIT ------------------------------------------------------------------------------------------------------- //

TgINLINE TgVOID V4(F_QT_INIT_AXS_V3)( V4(PCU_TgVEC) ptvR0, V3(CPCU_TgVEC) ptvAxis, const TYPE tyAngle )
{
    TYPE                                tySinA, tyCosA;

    TgASSERT( F(tgCM_NR1)( V3(F_LEN)( ptvAxis ) ) && !F(tgCM_NaN)( tyAngle ) );

    F(tgPM_SINCOS)( &tySinA, &tyCosA, MKL(0.5)*tyAngle );
    *ptvR0 = V4(F_SET_ELEM)( ptvAxis->m_aData[0]*tySinA, ptvAxis->m_aData[1]*tySinA, ptvAxis->m_aData[2]*tySinA, tyCosA );
}


TgINLINE TgVOID V4(F_QT_INIT_AXS_V4)( V4(PCU_TgVEC) ptvR0, V4(CPCU_TgVEC) ptvAxis, const TYPE tyAngle )
{
    TYPE                                tySinA,tyCosA;

    TgASSERT( F(tgCM_NR1)( V4(F_LEN)( ptvAxis ) ) && !F(tgCM_NaN)( tyAngle ) )

    F(tgPM_SINCOS)( &tySinA, &tyCosA, MKL(0.5)*tyAngle );
    *ptvR0 = V4(F_SET_ELEM)( ptvAxis->m_aData[0]*tySinA, ptvAxis->m_aData[1]*tySinA, ptvAxis->m_aData[2]*tySinA, tyCosA );
}


TgINLINE TgVOID V4(F_QT_INIT_AXS)( V4(PCU_TgVEC) ptvR0, V4(CPCU_TgVEC) ptvAA )
{
    TYPE                                tySinA,tyCosA;
    #if !defined(NDEBUG)
      V3(C_TgVEC)                           tvTest = V3(F_SET_ELEM)( ptvAA->m_aData[0], ptvAA->m_aData[1], ptvAA->m_aData[2] );
      TgASSERT( F(tgPM_ABS)( V3(F_LEN)( &tvTest ) - MKL(1.0) ) < F(TgEPS) )
    #endif
    TgASSERT( F(tgCM_NR1)( V3(F_LEN)( (V3(P_TgVEC))ptvAA ) ) && !F(tgCM_NaN)( ptvAA->m_aData[3] ) )
    F(tgPM_SINCOS)( &tySinA, &tyCosA, MKL(0.5)*ptvAA->m_aData[3] );
    *ptvR0 = V4(F_SET_ELEM)( ptvAA->m_aData[0]*tySinA, ptvAA->m_aData[1]*tySinA, ptvAA->m_aData[2]*tySinA, tyCosA );
}


TgINLINE TgVOID V4(F_QT_INIT_EUL)( V4(PCU_TgVEC) ptvR0, const TYPE tyX, const TYPE tyY, const TYPE tyZ )
{
    TYPE                                tySinZ,tyCosZ, tySinX,tyCosX, tySinY,tyCosY; // [ROLL, PITCH, YAW]
    register TYPE                       tyTA, tyTB, tyTC, tyTD;

    F(tgPM_SINCOS)( &tySinX, &tyCosX, MKL(0.5)*tyX ); //« Roll
    F(tgPM_SINCOS)( &tySinY, &tyCosY, MKL(0.5)*tyY ); //« Pitch
    F(tgPM_SINCOS)( &tySinZ, &tyCosZ, MKL(0.5)*tyZ ); //« Yaw

    tyTA = tySinX*tyCosY;
    tyTB = tyCosX*tySinY;
    tyTC = tyCosX*tyCosY;
    tyTD = tySinX*tySinY;

    *ptvR0 = V4(F_SET_ELEM)( tyTA*tyCosZ - tyTB*tySinZ, tyTB*tyCosZ + tyTA*tySinZ,
        tyTC*tySinZ - tyTD*tyCosZ, tyTC*tyCosZ + tyTD*tySinZ );
}


TgINLINE TgVOID V4(F_QT_INIT_ELX)( V4(PCU_TgVEC) ptvR0, const TYPE tyX )
{
    TYPE                                tySinX,tyCosX;

    F(tgPM_SINCOS)( &tySinX, &tyCosX, MKL(0.5)*tyX );
    *ptvR0 = V4(F_SET_ELEM)( tySinX, MKL(0.0), MKL(0.0), tyCosX );
}


TgINLINE TgVOID V4(F_QT_INIT_ELY)( V4(PCU_TgVEC) ptvR0, const TYPE tyY )
{
    TYPE                                tySinY,tyCosY;

    F(tgPM_SINCOS)( &tySinY, &tyCosY, MKL(0.5)*tyY );
    *ptvR0 = V4(F_SET_ELEM)( MKL(0.0), tySinY, MKL(0.0), tyCosY );
}


TgINLINE TgVOID V4(F_QT_INIT_ELZ)( V4(PCU_TgVEC) ptvR0, const TYPE tyZ )
{
    TYPE                                tySinZ,tyCosZ;

    F(tgPM_SINCOS)( &tySinZ, &tyCosZ, MKL(0.5)*tyZ );
    *ptvR0 = V4(F_SET_ELEM)( MKL(0.0), MKL(0.0), tySinZ, tyCosZ );
}




// ---- QUATERNION - EULER ------------------------------------------------------------------------------------------------------ //

TgINLINE TgVOID F(F_Quat2Eul)( PCU_TYPE ptyX, PCU_TYPE ptyY, PCU_TYPE ptyZ, V4(CPCU_TgVEC) ptqR0 )
{
    const TYPE tySinY = MKL(2.0) * (ptqR0->m_aData[1]*ptqR0->m_aData[3] - ptqR0->m_aData[0]*ptqR0->m_aData[2]);

    if (MKL(1.0) + tySinY <= F(TgEPS))
    {
        *ptyX = MKL(2.0) * F(tgPM_ATAN2)( ptqR0->m_aData[2], ptqR0->m_aData[3] );
        *ptyY =-F(TgKT_HALF_PI);
        *ptyZ = MKL(0.0);
    }
    else if (tySinY >= MKL(1.0) - F(TgEPS))
    {
        *ptyX = MKL(2.0) * F(tgPM_ATAN2)( ptqR0->m_aData[0], ptqR0->m_aData[3] );
        *ptyY = F(TgKT_HALF_PI);
        *ptyZ = MKL(0.0);
    }
    else
    {
        const TYPE tyTMPA = MKL(2.0)*(ptqR0->m_aData[1]*ptqR0->m_aData[2] + ptqR0->m_aData[3]*ptqR0->m_aData[0]);
        const TYPE tyTMPB = MKL(2.0)*(ptqR0->m_aData[0]*ptqR0->m_aData[0] + ptqR0->m_aData[1]*ptqR0->m_aData[1]);
        const TYPE tyTMPC = MKL(2.0)*(ptqR0->m_aData[0]*ptqR0->m_aData[1] + ptqR0->m_aData[2]*ptqR0->m_aData[3]);
        const TYPE tyTMPD = MKL(2.0)*(ptqR0->m_aData[1]*ptqR0->m_aData[1] + ptqR0->m_aData[2]*ptqR0->m_aData[2]);

        *ptyX = F(tgPM_ATAN2)(tyTMPA, MKL(1.0) - tyTMPB);
        *ptyY = F(tgPM_ASIN)(tySinY);
        *ptyZ = F(tgPM_ATAN2)(tyTMPC, MKL(1.0) - tyTMPD);
    };
}


TgINLINE TgVOID V4(F_Eul2Quat)( V4(PCU_TgVEC) ptqR0, const TYPE tyX, const TYPE tyY, const TYPE tyZ )
{
    TYPE                                tySinZ,tyCosZ, tySinX,tyCosX, tySinY,tyCosY; // [ROLL, PITCH, YAW]

    F(tgPM_SINCOS)( &tySinX, &tyCosX, MKL(0.5)*tyX ); //« Roll
    F(tgPM_SINCOS)( &tySinY, &tyCosY, MKL(0.5)*tyY ); //« Pitch
    F(tgPM_SINCOS)( &tySinZ, &tyCosZ, MKL(0.5)*tyZ ); //« Yaw

    {
        register const TYPE                 tyTA = tySinX*tyCosY;
        register const TYPE                 tyTB = tyCosX*tySinY;
        register const TYPE                 tyTC = tyCosX*tyCosY;
        register const TYPE                 tyTD = tySinX*tySinY;

        ptqR0->m.x = tyTA*tyCosZ - tyTB*tySinZ;
        ptqR0->m.y = tyTB*tyCosZ + tyTA*tySinZ;
        ptqR0->m.z = tyTC*tySinZ - tyTD*tyCosZ;
        ptqR0->m.w = tyTC*tyCosZ + tyTD*tySinZ;
    }
}


TgINLINE TgVOID V3(F_Quat2Eul_V4)( V3(PCU_TgVEC) ptvEul, V4(CPCU_TgVEC) ptqR0 )
{
    F(F_Quat2Eul)( &ptvEul->m.x, &ptvEul->m.y, &ptvEul->m.z, ptqR0 );
}


TgINLINE TgVOID V4(F_Eul2Quat_V3)( V4(PCU_TgVEC) ptqR0, V3(CPCU_TgVEC) ptvEul )
{
    V4(F_Eul2Quat)( ptqR0, ptvEul->m.x, ptvEul->m.y, ptvEul->m.z );
}


TgINLINE TgVOID V4(F_Quat2Eul_V4)( V4(PCU_TgVEC) ptvEul, V4(CPCU_TgVEC) ptqR0 )
{
    F(F_Quat2Eul)( &ptvEul->m.x, &ptvEul->m.y, &ptvEul->m.z, ptqR0 );
}


TgINLINE TgVOID V4(F_Eul2Quat_V4)( V4(PCU_TgVEC) ptqR0, V4(CPCU_TgVEC) ptvEul )
{
    V4(F_Eul2Quat)( ptqR0, ptvEul->m.x, ptvEul->m.y, ptvEul->m.z );
}




// ---- QUATERNION - FUNCTIONS -------------------------------------------------------------------------------------------------- //

TgINLINE TgVOID V3(F_QT_VECTOR_TO_VECTOR)( V4(PCU_TgVEC) ptvR0, V3(CPCU_TgVEC) ptvFrom, V3(CPCU_TgVEC) ptvTo )
{
    // If either the scale or the dot product are invalid this will cause a return of the zero quaternion.

    const TYPE                          tyFm_To = V3(F_DOT3_VV)( ptvFrom, ptvTo ); //« cos(θ) = A*B/|A||B|
    const TYPE                          tyScale = V3(F_LEN)( ptvFrom )*V3(F_LEN)( ptvTo );
    TYPE                                tyCosA = F(tgPM_FSEL)( tyScale - F(TgEPS), tyFm_To / tyScale, MKL(1.0) );
    V3(C_TgVEC)                        tvAxis = V3(F_UCX)( ptvFrom, ptvTo );
    const TYPE                          tySinHA = F(tgPM_SQRT)((MKL(1.0) - tyCosA)*MKL(0.5));
    const TYPE                          tyLimit = F(tgCM_CLP)( tyCosA, MKL(1.0), MKL(-1.0) ); 

    // Ensure that the result is within function range (floating point error)
    // Axis of rotation would be the vector perpendicular to both inputs, normalized if necessary
    // For an axis-angle, need to generate cos(θ/2) and sin(θ/2)
    //  Trig Identity: cos(2θ) = cos²(θ) - sin²(θ) = 2*cos²(θ) - 1 = 1 - 2*sin²(θ)
    //                 cos(θ/2) = √((1 + cos(θ))/2)

    ptvR0->m_aData[0] = tvAxis.m_aData[0] * tySinHA;
    ptvR0->m_aData[1] = tvAxis.m_aData[1] * tySinHA;
    ptvR0->m_aData[2] = tvAxis.m_aData[2] * tySinHA;
    ptvR0->m_aData[3] = F(tgPM_SQRT)((MKL(1.0) + tyLimit)*MKL(0.5));
}